/**
  ******************************************************************************
  * @file    sg90.c 
  * @author  https://gitee.com/hyk-aajjjj33
  * @version V1.0.0
  * @date    2024-12-26
  * @brief   舵机控制
  ******************************************************************************
  * @attention
  *            
  * 
  *
  ******************************************************************************
  
**/

/* Includes ------------------------------------------------------------------*/
#include "sg90.h"


/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
volatile uint32_t sg1=90;
volatile uint32_t sg2=90;
volatile uint32_t sg3=90;
volatile uint32_t sg4=90;
volatile uint32_t sg5=90;
volatile uint32_t sg6=90;
/* Private function prototypes -----------------------------------------------*/
/* rivate functions ---------------------------------------------------------*/


void sg90_init(void)
{
	GPIO_InitTypeDef GPIO_InitStruct;
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD,ENABLE);//使能PA时钟
	
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;//模拟输入
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_1 | GPIO_Pin_2| GPIO_Pin_3| GPIO_Pin_4| GPIO_Pin_5| GPIO_Pin_6;
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOD, &GPIO_InitStruct);
}


void sg90_reset(void)
{
	PWM_PUT(90,GPIO_Pin_1);		//右脚舵机
	PWM_PUT(90,GPIO_Pin_2);		//左脚舵机
	PWM_PUT(90,GPIO_Pin_3);		//右内舵机
	PWM_PUT(90,GPIO_Pin_4);		//左内舵机
	PWM_PUT(90,GPIO_Pin_5);   	//左手舵机
	PWM_PUT(90,GPIO_Pin_6);  	//右手舵机
	sg1=90;
	sg2=90;
	sg3=90;
	sg4=90;
	sg5=90;
	sg6=90;
}




void PWM_PUT(uint32_t angle,uint16_t GPIO_Pin)
{
	uint32_t put;
	uint8_t i=0;
	put=500+2000*angle/180;
	for(;i<1;i++)
	{
		GPIO_SetBits(GPIOD,GPIO_Pin);
		delay_us(put);
		GPIO_ResetBits(GPIOD,GPIO_Pin);
		delay_us(20000-put);
	}
}

void Slow_PUT_pin1(uint8_t angle)
{
    if(angle > sg1)
    {
        for(; sg1 <= angle; sg1+=3)  // 使用 sg1 递增
        {
            PWM_PUT(sg1, GPIO_Pin_1);    // 右脚舵机
        } 
    }
    else
    {																	
        for(; sg1 >= angle; sg1-=3)
        {
            PWM_PUT(sg1, GPIO_Pin_1);    // 右脚舵机
        }
    }
}

void Slow_PUT_pin2(uint8_t angle)
{
    if(angle > sg2)
    {
        for(; sg2 <= angle; sg2+=3)  // 使用 sg1 递增
        {
            PWM_PUT(sg2, GPIO_Pin_2);    // 右脚舵机
        }
    }
    else
    {																	
        for(; sg2 >= angle; sg2-=3)
        {
            PWM_PUT(sg2, GPIO_Pin_2);    // 右脚舵机
        }
    }
}

void Slow_PUT_pin3(uint8_t angle)
{
    if(angle > sg3)
    {
        for(; sg3 <= angle; sg3+=3)  // 使用 sg1 递增
        {
            PWM_PUT(sg3, GPIO_Pin_3);    // 右脚舵机
        }
    }
    else
    {																	
        for(; sg3 >= angle; sg3-=3)
        {
            PWM_PUT(sg3, GPIO_Pin_3);    // 右脚舵机
        }
    }
}

void Slow_PUT_pin4(uint8_t angle)
{
    if(angle > sg4)
    {
        for(; sg4<= angle; sg4+=3)  // 使用 sg1 递增
        {
            PWM_PUT(sg4, GPIO_Pin_4);    // 右脚舵机
        }
    }
    else
    {																	
        for(; sg4 >= angle; sg4-=3)
        {
            PWM_PUT(sg4, GPIO_Pin_4);    // 右脚舵机
        }
    }
}
void Slow_PUT_pin5(uint8_t angle)
{
    if(angle > sg5)
    {
        for(; sg5<= angle; sg5+=5)  // 使用 sg5递增
        {
            PWM_PUT(sg5, GPIO_Pin_5);    // 右脚舵机
        }
    }
    else
    {																	
        for(; sg5 >= angle; sg5-=5)
        {
            PWM_PUT(sg5, GPIO_Pin_5);    // 右脚舵机
        }
    }
}
void Slow_PUT_pin6(uint8_t angle)
{
    if(angle > sg6)
    {
        for(; sg6<= angle; sg6+=5)  // 使用 sg6递增
        {
            PWM_PUT(sg6, GPIO_Pin_6);    // 右脚舵机
        }
    }
    else
    {																	
        for(; sg6 >= angle; sg6-=5)
        {
            PWM_PUT(sg6, GPIO_Pin_6);    // 右脚舵机
        }
    }
}

//前进
void before(void)
{
	int i=0;
	for(;i<4;i++)
	{
		Slow_PUT_pin1(60);
		Slow_PUT_pin2(65);
		Slow_PUT_pin1(90);
		Slow_PUT_pin3(115);
		Slow_PUT_pin4(110);
		Slow_PUT_pin2(90);
		
		Slow_PUT_pin2(120);
		Slow_PUT_pin1(115);
		Slow_PUT_pin2(90);
		Slow_PUT_pin4(75);
		Slow_PUT_pin3(70);
		Slow_PUT_pin1(90);
	}
	sg90_reset();
	
}

//后退
void down(void)
{
	int i=0;
	for(;i<4;i++)
	{
		Slow_PUT_pin1(60);
		Slow_PUT_pin2(65);
		Slow_PUT_pin1(90);
		
		Slow_PUT_pin3(75);
		Slow_PUT_pin4(70);
		Slow_PUT_pin2(90);
		
		
		Slow_PUT_pin2(120);
		Slow_PUT_pin1(120);
		Slow_PUT_pin2(90);
		
		Slow_PUT_pin3(110);
		Slow_PUT_pin4(105);
		
		Slow_PUT_pin1(90);	
	}
	sg90_reset();
	
}

//右转
void right(void)
{
	int i=0;
	for(;i<4;i++)
	{
		Slow_PUT_pin1(60);
		Slow_PUT_pin2(65);
		Slow_PUT_pin1(90);
		Slow_PUT_pin3(120);                                                          	           
		Slow_PUT_pin2(90);
		
		Slow_PUT_pin2(120);
		Slow_PUT_pin1(115);
		Slow_PUT_pin3(90);
		Slow_PUT_pin2(90);
		Slow_PUT_pin1(90);
	}
	sg90_reset();
}

//左转
void lift(void)
{
	int i=0;
	for(;i<4;i++)
	{
		Slow_PUT_pin2(120);
		Slow_PUT_pin1(115);
		Slow_PUT_pin2(90);
		Slow_PUT_pin4(70);
		Slow_PUT_pin1(90);
		
		Slow_PUT_pin1(60);
		Slow_PUT_pin2(65);
		Slow_PUT_pin1(90);
		Slow_PUT_pin4(90);
		Slow_PUT_pin2(90);
		
	}
	sg90_reset();
	
}




//举手右手
void  left_hand(void)
{
	PWM_PUT(0,GPIO_Pin_6);
}



//跳舞
void Dancing(void)
{
	Slow_PUT_pin1(60);
	Slow_PUT_pin2(60);
	Slow_PUT_pin5(90);
	delay_s(2);
	Slow_PUT_pin6(180);
	
	
}

//原地踏步
void fun1()
{
	int i=0;
	for(;i<5;i++)
	{
		Slow_PUT_pin1(60);
		Slow_PUT_pin2(65);
		Slow_PUT_pin1(90);
		Slow_PUT_pin3(115);
		Slow_PUT_pin2(90);
		
		Slow_PUT_pin2(120);
		Slow_PUT_pin1(115);
		Slow_PUT_pin2(90);
		Slow_PUT_pin1(90);
	}
	sg90_reset();
}
